package org.team751.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.command.PIDSubsystem;

/**
 * Base class for a segment (top or bottom) of the gripper.<br />
 * Implements everything except the constructor with the specific I/O stuff
 * @author Programmer
 */
public abstract class GripperWheel extends PIDSubsystem implements GripperPIDConstants {
    protected Encoder encoder;
    protected Jaguar motor;

    public GripperWheel(String name, double p, double i, double d) {
        super(name, p, i, d);
    }

    /**
     * Set the PID controller to target a value relative to the current target.<br />
     * A positive value will make the wheels rotate to pull a game piece in.<br />
     * If PID has been disabled by calling setManual(), it will be immediately
     * re-enabled and the new target will be relative to the actual current location.
     * @param degrees The number of degrees to rotate
     */
    public void incrementTarget(int degrees) {
        //Check if PID is disabled
        if (!getPIDController().isEnable()) {
            //PID disabled; about to enable it:
            //set the target to the current actual location
            setSetpoint(getPosition());
            //now enable it
            enable();
        }
        double target = getSetpoint();
        target += degrees;
        System.out.println(toString()+": Current position="+getPosition()+" New target = "+target);
        setSetpointRelative(degrees);
        enable();
    }

    protected void initDefaultCommand() {
        
    }

    protected double returnPIDInput() {
        int input = encoder.get();
        System.out.println(toString()+": Giving input "+input+" to PID");
        return input;
    }

    /**
     * Set the motor to a given speed manually, overriding PID.<br />
     * After this is called, PID is disabled until a PID-related function is called.
     * @param speed The ratio of speed to apply to the motor, with positive values
     * pulling the game piece in
     */
    public void setManual(double speed) {
        disable();
        motor.set(speed);
    }

    protected void usePIDOutput(double output) {
        System.out.println(toString()+": Setting output to "+output);
        motor.set(output);
    }
    
}
